package optimization;

import static optimization.LinearAlgebra.*;

import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
import java.util.logging.Level;
import java.util.logging.Logger;

import theory.vanpelt.bifurcation.PDistEstimate;


public class NelderMead {
	
	class PointValue implements Comparable<PointValue> {
		double[] x;
		double fx;
		public PointValue(double[] p) {
			this.x = p;
		}
		@Override
		public int compareTo(PointValue o) {
			return Double.compare(fx, o.fx);
		}
	}

	/**
	 * Simplex points
	 */
	List<PointValue> points;
	
	/**
	 * Best, second-worst, worst
	 */
	double fl, fs, fh; 

	/**
	 * centroid
	 */
	double[] c;
	
	/**
	 * reflection point
	 */
	double[] xr;
	double fr;

	/**
	 * Constants
	 */
	protected double alpha = 1, beta = 0.5, gamma = 2, delta = 0.5;
	
	/**
	 * Function (value)
	 */
	StochasticFunction f;

	/**
	 * Constraints mapping
	 */
	VectorFunction cons;
	
	/**
	 * Dennis-Woods termination threshold
	 */
	double nu = 0.0001;

	
	int N=1;
	long seed=0;
	
	public NelderMead(StochasticFunction f, VectorFunction cons) {
		super();
		this.f = f;
		this.cons = cons;
	}
	
	public NelderMead(StochasticFunction f, VectorFunction cons, double terminationThreshold) {
		super();
		this.f = f;
		this.cons = cons;
		this.nu = terminationThreshold;
	}
	
	public void setStochasticParameters(int N, long seed)
	{
		this.N = N;
		this.seed = seed;
	}

	protected boolean reflect()
	{
		if (fl <= fr && fr < fs) {
			System.out.println("R");
			points.get(2).x = xr;
			points.get(2).fx = fr;
			return true;
		}
		return false;
	}

	protected boolean expand()
	{
		if (fr < fl) {
			double[] xe = cons.evaluate(vAdd(c, vMul(getGamma(), vSub(xr, c))));
			double fe = f.evaluate(xe, N, seed);
			if (fe < fr) {
				System.out.println("E1");
				points.get(2).x = xe;
				points.get(2).fx = fe;
				return true;
			} else {
				System.out.println("ER");
				points.get(2).x = xr;
				points.get(2).fx = fr;
				return true;
			}
		}
		return false;
	}

	protected boolean contract()
	{
		if (fr >= fs) {

			// outside
			if (fs <= fr && fr < fh) {
				double[] xc = cons.evaluate(vAdd(c, vMul(getBeta(), vSub(xr, c))));
				double fc = f.evaluate(xc, N, seed);
				if (fc <= fr) {
					System.out.println("CO");
					points.get(2).x = xc;
					points.get(2).fx = fc;
					return true;
				}
			}
			
			// inside
			else if (fr >= fh) {
				double[] xh = points.get(2).x;
				double[] xc = cons.evaluate(vAdd(c, vMul(getBeta(), vSub(xh, c))));
				double fc = f.evaluate(xc, N, seed);
				if (fc < fh) {
					System.out.println("CI");
					points.get(2).x = xc;
					points.get(2).fx = fc;
					return true;
				}
			}
			
			// shrink
			return shrink();
		}
		return false;
	}

	protected boolean shrink()
	{
		System.out.println("S");
		double[] xs1 = cons.evaluate(vAdd(points.get(0).x, vMul(getDelta(), vSub(points.get(1).x, points.get(0).x))));
		double fs1 = f.evaluate(xs1, N, seed);
		double[] xs2 = cons.evaluate(vAdd(points.get(0).x, vMul(getDelta(), vSub(points.get(2).x, points.get(0).x))));
		double fs2 = f.evaluate(xs2, N, seed);
		points.get(1).x = xs1;
		points.get(1).fx = fs1;
		points.get(2).x = xs2;
		points.get(2).fx = fs2;
		return true;		
	}
		
	protected boolean dxTerminationTest()
	{
		double dx01 = vNorm2(vSub(points.get(0).x, points.get(1).x));
		double dx02 = vNorm2(vSub(points.get(0).x, points.get(2).x));
		double dx12 = vNorm2(vSub(points.get(1).x, points.get(2).x));
		double dx = dx01+dx02+dx12;
		if (dx < 0.01) return true; 
		return false;
	}
	
	protected boolean dennisWoodsTerminationTest()
	{
		double normX0 = Math.max(1,vNorm2(points.get(0).x));
		double maxNorm = Double.MIN_VALUE;
		for (int i = 1; i < 3; i++) {
			double norm = vNorm2(vSub(points.get(0).x, points.get(i).x));
			if (norm > maxNorm) maxNorm = norm;
		}
		if (maxNorm / normX0 < nu) return true;
		return false;
	}
	
	public double[] search(double[] x0i, double[] h0) 
	{	
		points = new ArrayList<NelderMead.PointValue>();
		
		points.add(new PointValue(x0i));
		points.add(new PointValue(vAdd(x0i, vSelect(h0, 0))));
		points.add(new PointValue(vAdd(x0i, vSelect(h0, 1))));
		for (int i = 0; i < 3; i++) points.get(i).fx = f.evaluate(points.get(i).x, N, seed);

		int MAX_ITER = 5000;
		
		for (int iter = 0; iter < MAX_ITER; iter++) {

			// sort by value
			Collections.sort(points);
			fl = points.get(0).fx; // best        0
			fs = points.get(1).fx; // 2nd worst   n-2
			fh = points.get(2).fx; // worst       n-1

			// termination tests
//			if (dxTerminationTest()) return points.get(0).x;
			if (dennisWoodsTerminationTest()) return points.get(0).x;
			
			Logger.getLogger("com.ristolaakso").logp(Level.INFO, Thread.currentThread().getStackTrace()[0].getClassName(), Thread.currentThread().getStackTrace()[0].getMethodName(),
					"NM"+iter + "   " + vPrint(points.get(0).x)+ ", " + vPrint(points.get(1).x) + ", " + vPrint(points.get(2).x));
//			System.out.print("NM"+iter + "   " + vPrint(points.get(0).x)+ ", " + vPrint(points.get(1).x) + ", " + vPrint(points.get(2).x));
			
			// compute centroid
			c = vAdd(points.get(0).x, points.get(1).x);
			c = vMul(1.0/2.0, c);
			
			Logger.getLogger("com.ristolaakso").logp(Level.INFO, Thread.currentThread().getStackTrace()[0].getClassName(), Thread.currentThread().getStackTrace()[0].getMethodName(),
				String.format("   %.5f, %.5f, %.5f  -> ", fl, fs, fh));
			
			// compute reflection point
			xr = cons.evaluate(vAdd(c, vMul(getAlpha(), vSub(c, points.get(2).x))));
			fr = f.evaluate(xr, N, seed);
				
			// reflection
			boolean tmp = reflect() || expand() || contract();
/*			
			if (reflect()) { }
			
			// expand
			else if (expand()) { }
			
			// contract or shrink
			else { contract(); break; }
	*/			
		} // ITER
		
		return points.get(0).x;
	}

	
	
	public static void main(String[] args) {
		StochasticFunction f = new StochasticFunction() {
			
			@Override
			public double evaluate(double[] p, int N, long seed) {
				PDistEstimate pd = new PDistEstimate(p[0], p[1]);
				double dm = pd.getMeanEst() - 5.35;
				double ds = pd.getDevEst() - 4.41;
				return Math.sqrt(dm*dm+ds*ds);
			}
		};

		VectorFunction cons = new VectorFunction() {			
			@Override
			public int yDim() {
				return 2;
			}
			@Override
			public double[] evaluate(double[] p) {
				if (p[0] < 1) p[0] = 1;
				if (p[0] > 6) p[0] = 6;
				if (p[1] < 0) p[1] = 0;
				if (p[1] > 1) p[1] = 1;
				return p;
			}
		};
		
		NelderMead nm = new NelderMead(f, cons, 0.01);
		double[] xstar = nm.search(new double[] { 2.5, 0.5 }, new double[] { 0.1, 0.05 });
		PDistEstimate pd = new PDistEstimate(xstar[0], xstar[1]);
		System.out.println(vPrint(xstar) + " -> " + f.evaluate(xstar,1,0) + " == " + pd.getMeanEst() + ", " + pd.getDevEst());
	}

	protected double getAlpha() {
		return alpha;
	}

	protected double getBeta() {
		return beta;
	}

	protected double getGamma() {
		return gamma;
	}

	protected double getDelta() {
		return delta;
	}
}
